#include <util/delay.h>
#include <avr/interrupt.h> 
#include <avr/wdt.h>

#include "hc.h"

#include "mouses.h"
#include "utils.h"

#define FOSC 16000000 // 16 MHz quartz
#define BAUD 19200
#define MYUBRR (FOSC/16/BAUD-1)

volatile uint8_t flag=1;
volatile int16_t good0Bytes, bad0Bytes, good1Bytes, bad1Bytes;
	
int main() {

	// init PWMs
	TCCR1B = _BV(CS11) | _BV(WGM13);
	ICR1 = PWM_TOP_VALUE;  

	// init servo PWM
	DDRB |= _BV(PB2);
	TCCR1A |= _BV(COM1B1);
	SET_RUDDER_POS(RUDDER_CENTER_POS);

	// init thrust PWM 
	DDRB |= _BV(PB1);
	TCCR1A |= _BV(COM1A1);
	SET_THRUST_POWER(MIN_THRUST_POWER);


	// init lift
	DDRB |= _BV(PB0);
	TURN_LIFT_OFF();

	_delay_ms(10);

	// init USART
	UBRRH = (unsigned char)(MYUBRR>>8);
	UBRRL = (unsigned char)MYUBRR;
	UCSRB = (1<<RXEN) | (1<<TXEN) | (1<<TXCIE);
	UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<<UCSZ0);

	sei();
	initMouses();

	uint8_t data[3];

#define PACKET_SIZE 2
	uint8_t bytesToSend[PACKET_SIZE];
	
	int16_t mouse0PosX=0, mouse0PosY=0;
	int16_t mouse1PosX=0, mouse1PosY=0;
	uint8_t trCtl=TRCTL_BIT, sendingByte=0, gotRequest=0;
	uint8_t isRudderMoving=0, isRudderMovingLeft=0, movingPauseCounter=0;
	volatile uint16_t pos;
	

	for(;;) {
	
		movingPauseCounter++;
		if (isRudderMoving && movingPauseCounter % RUDDER_MOVING_STEP_INTERVAL == 0 ) {
			movingPauseCounter=0;
			pos = GET_RUDDER_POS();
			if (isRudderMovingLeft && pos>RUDDER_MAX_LEFT_POS) SET_RUDDER_POS(pos-1);
			else if (!isRudderMovingLeft && pos<RUDDER_MAX_RIGHT_POS) SET_RUDDER_POS(pos+1);
		}

		if (getMouse0Data(data)) {
			bytesToSend[1]=data[0];
			
			if (data[0] & X_SIGN_BIT) { /* bytesToSend[8]=0xFF; bytesToSend[9]=data[1]; */ data[1] = (~data[1])+1; mouse0PosX -= data[1];}
			else { mouse0PosX += data[1]; /* bytesToSend[8]=0; bytesToSend[9]=data[1];*/}

			if (data[0] & Y_SIGN_BIT) { /* bytesToSend[10]=0xFF; bytesToSend[11]=data[2]; */ data[2] = (~data[2])+1; mouse0PosY -= data[2];}
			else { mouse0PosY += data[2]; /* bytesToSend[10]=0; bytesToSend[11]=data[2];*/}
			
		}
		if (getMouse1Data(data)) {
			//bytesToSend[19]=data[0];

			if (data[0] & X_SIGN_BIT) { /* bytesToSend[12]=0xFF; bytesToSend[13]=data[1]; */ data[1] = (~data[1])+1; mouse1PosX -= data[1];}
			else { mouse1PosX += data[1]; /* bytesToSend[12]=0; bytesToSend[13]=data[1]; */}
			
			if (data[0] & Y_SIGN_BIT) { /* bytesToSend[14]=0xFF; bytesToSend[15]=data[2]; */ data[2] = (~data[2])+1; mouse1PosY -= data[2];}
			else { mouse1PosY += data[2]; /*bytesToSend[14]=0; bytesToSend[15]=data[2];*/ }
		}
		
		// if new byte has been recieved
		if(UCSRA & (1 << RXC)) {
			uint8_t rcv;
			rcv = UDR;
			gotRequest=1;
			
			if ((rcv & TURNING_BITS) == TURNING_BITS) {
				wdt_enable(WDTO_15MS);
				_delay_ms(100);
			}
			
			// turn lift on/off
			if (rcv & LIFT_BIT) TURN_LIFT_ON();
			else TURN_LIFT_OFF();
			
			// turn track controll on/off
			trCtl = rcv & TRCTL_BIT;
			
			// set thrust power
			uint8_t speed=0;
			if (rcv & THRUST_BIT0) speed += THRUST_SPEED_1;
			if (rcv & THRUST_BIT1) speed += THRUST_SPEED_2;
			SET_THRUST_POWER(speed*100);
			
			// set rudder pos
			int16_t turn=0;
			if (rcv & TURN_BIT0) turn += TURNING_RUDDER_POS_1;
			if (rcv & TURN_BIT1) turn += TURNING_RUDDER_POS_2;
			isRudderMovingLeft = rcv & TURN_DIRECTION_BIT;
			isRudderMoving = rcv & TURN_BIT2;
			if (!isRudderMoving && (rcv & TURNING_BITS) != TURN_DIRECTION_BIT) {
				if (rcv & TURN_DIRECTION_BIT) turn = -turn;
				SET_RUDDER_POS(RUDDER_CENTER_POS+turn);
			}
		}

		//bytesToSend[0]=mouse0PosX>>8;
		//bytesToSend[1]=mouse0PosX;
		//bytesToSend[2]=mouse0PosY>>8;
		//bytesToSend[3]=mouse0PosY;
		//bytesToSend[4]=mouse1PosX>>8;
		//bytesToSend[5]=mouse1PosX;
		//bytesToSend[6]=mouse1PosY>>8;
		//bytesToSend[7]=mouse1PosY;

		//bytesToSend[20]=good0Bytes>>8;
		//bytesToSend[21]=good0Bytes;
		//bytesToSend[22]=bad0Bytes>>8;
		//bytesToSend[23]=bad0Bytes;
		//bytesToSend[24]=good1Bytes>>8;
		//bytesToSend[25]=good1Bytes;
		//bytesToSend[26]=bad1Bytes>>8;
		//bytesToSend[27]=bad1Bytes;
		
		bytesToSend[0]=getRudderAngle();
		
		if (flag && gotRequest) {
			flag=0;
			UDR = bytesToSend[sendingByte];
			sendingByte=(sendingByte+1)%PACKET_SIZE;
			if (sendingByte == 0) gotRequest=0;
		}
	}
}


ISR(USART_TXC_vect) {
	flag=1;
}

volatile inline int8_t getRudderAngle() {
	return (int8_t)((signed)(GET_RUDDER_POS() - RUDDER_CENTER_POS)/RUDDER_POS_TO_DEGREE_RATIO);
}